// task_action_server.cpp
#include "task_action_server.h"
#include <future>
#include <QThread>
#include <QCoreApplication>
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>
#include <QJsonArray>
#include <QDir>
#include <QDateTime>
#include <QTimer>
#include <rclcpp/client.hpp>
#include <fstream>
#include <sstream>
#include <iomanip>
#include "../include/seer/SCHeadData.h"

/**
 * 创建一个任务Action服务器节点，并初始化导航和状态套接字
 */
TaskServerNode::TaskServerNode()
    : QObject()
    , Node("task_action_server")
    , m_naviSocket(nullptr)
    , m_confSocket(nullptr)
    , m_yoloSocket(nullptr)  // 初始化YOLO socket
    , m_requestManager(nullptr)
    , m_reconnectTimer(nullptr)
    , m_healthCheckTimer(nullptr)
    , m_wasConnected(false)
    , hasControlAuthority(false)
    , dbManager_(std::make_shared<DatabaseManager>(this))
{

    LOG_INFO("TaskServerNode 初始化开始");
    
    // 初始化机器人连接
    m_naviSocket = new SCStatusTcp(this);   //SRC2000导航Socket
    m_confSocket = new SCStatusTcp(this);   //SRC2000配置Socket（抢占控制权）
    m_otherSocket = new SCStatusTcp(this);  // SRC2000其他Socket(设置DO控制喷药)
    m_yoloSocket = new SCStatusTcp(this);   // YOLO图像处理任务Socket
    m_requestManager = new RequestManager(this);
    
    // 从参数获取机器人IP地址，默认为192.168.1.150
    std::string seer_ip = this->declare_parameter("seer_ip", "192.168.1.150");
    std::string yolo_ip = this->declare_parameter("yolo_ip", "127.0.0.1");      // YOLO任务IP地址参数
    m_naviPort = this->declare_parameter("navi_port", SEER_API_PORT_NAVI);
    m_confPort = this->declare_parameter("conf_port", SEER_API_PORT_CONF);
    m_otherPort = this->declare_parameter("other_port", SEER_API_PORT_OTHER);   // SRC2000其他端口参数
    m_yoloPort = this->declare_parameter("yolo_port", RMS_TASK_YOLO_PORT);      // YOLO端口参数
    spraying_gpio_id_ = this->declare_parameter("spraying_gpio_id", 0);         // 默认喷药控制使用SRC2000 GPIO编号为0

    //获取参数
    m_seerIP = QString::fromStdString(seer_ip);
    m_yoloIP = QString::fromStdString(yolo_ip);  // 获取YOLO IP地址
    
    LOG_INFO("获取参数完成: seer_ip={}, navi_port={}, stat_port={}, yolo_ip={}, yolo_port={}", 
                 seer_ip, m_naviPort, m_confPort, yolo_ip, m_yoloPort);
    
    // 初始化连接
    initializeConnections();

    // 连接信号槽
    connect(m_naviSocket->tcpSocket(), &QTcpSocket::stateChanged, 
            this, &TaskServerNode::handleSocketStateChanged);
    connect(m_confSocket->tcpSocket(), &QTcpSocket::stateChanged, 
            this, &TaskServerNode::handleSocketStateChanged);
    connect(m_otherSocket->tcpSocket(), &QTcpSocket::stateChanged,  // 连接SRC2000其他socket状态变化信号
            this, &TaskServerNode::handleSocketStateChanged);
    connect(m_yoloSocket->tcpSocket(), &QTcpSocket::stateChanged,  // 连接YOLO socket状态变化信号
            this, &TaskServerNode::handleSocketStateChanged);
    connect(m_naviSocket->tcpSocket(), &QTcpSocket::errorOccurred, 
            this, &TaskServerNode::handleSocketError);
    connect(m_confSocket->tcpSocket(), &QTcpSocket::errorOccurred, 
            this, &TaskServerNode::handleSocketError);
    connect(m_otherSocket->tcpSocket(), &QTcpSocket::errorOccurred,  // 连接SRC2000其他socket错误信号
            this, &TaskServerNode::handleSocketError);
    connect(m_yoloSocket->tcpSocket(), &QTcpSocket::errorOccurred,  // 连接YOLO socket错误信号
            this, &TaskServerNode::handleSocketError);
    
    // 连接控制权获取信号
    connect(this, &TaskServerNode::controlAuthorityAcquired, 
            this, &TaskServerNode::onControlAuthorityAcquired);
    
    // 连接任务命令发送完成信号
    connect(this, &TaskServerNode::taskCommandSent,
            this, &TaskServerNode::onTaskCommandSent);
    
    // 创建健康检查定时器
    m_healthCheckTimer = new QTimer(this);
    m_healthCheckTimer->setInterval(30000); // 30秒检查一次
    connect(m_healthCheckTimer, &QTimer::timeout, this, &TaskServerNode::healthCheckTimeout);
    m_healthCheckTimer->start();
    LOG_INFO("健康检查定时器已启动，间隔: 30秒");

    // 获取节点命名空间
    std::string node_namespace = this->get_namespace();
    std::string service_name;
    
    // 如果命名空间是根命名空间，则直接使用服务名
    if (node_namespace == "/") {
        service_name = "get_robot_status";
    } else {
        // 否则拼接命名空间和服务名
        service_name = node_namespace + "/get_robot_status";
        // 从命名空间中提取服务名称
        //m_acquirerControlName = node_namespace.substr(1); // 移除开头的 "/"
    }
    
    // 创建RobotStatus服务客户端
    m_robotStatusClient = this->create_client<rms_pkg::srv::RobotStatus>(service_name);
    LOG_INFO("创建服务客户端: {}", service_name);

    // 初始化数据库
    if (!dbManager_->initialize("rms_client.db")) {
        LOG_ERROR("Failed to initialize database: {}", dbManager_->lastError().toStdString());
    } else {
        LOG_INFO("数据库初始化成功");
        
        // 创建传感器数据表
        QString sensorTableDefinition = "id INTEGER PRIMARY KEY AUTOINCREMENT, "
                                       "robot_id TEXT, "
                                       "timestamp TEXT, "
                                       "air_temperature REAL, "
                                       "air_humidity REAL, "
                                       "light_intensity REAL, "
                                       "co2_concentration INTEGER, "
                                       "soil_temperature REAL, "
                                       "soil_moisture REAL";
        if (!dbManager_->createTable("sensor_data", sensorTableDefinition)) {
            LOG_ERROR("Failed to create sensor_data table: {}", dbManager_->lastError().toStdString());
        }
        
        // 创建任务结果表
        QString taskTableDefinition = "id INTEGER PRIMARY KEY AUTOINCREMENT, "
                                     "task_id TEXT UNIQUE, "
                                     "status TEXT, "
                                     "message TEXT, "
                                     "completion_percentage INTEGER, "
                                     "start_time TEXT, "
                                     "end_time TEXT";
        if (!dbManager_->createTable("task_results", taskTableDefinition)) {
            LOG_ERROR("Failed to create task_results table: {}", dbManager_->lastError().toStdString());
        }
    }

    action_server_ = rclcpp_action::create_server<TaskExecution>(
        this,
        "execute_task",
        std::bind(&TaskServerNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
        std::bind(&TaskServerNode::handle_cancel, this, std::placeholders::_1),
        std::bind(&TaskServerNode::handle_accepted, this, std::placeholders::_1)
    );

     LOG_INFO("导航服务启动成功：等待平动或导航任务...");
}

/**
 * 初始化机器人连接
 */
void TaskServerNode::initializeConnections()
{
    LOG_INFO("初始化机器人连接");
    
    // 创建重连定时器
    m_reconnectTimer = new QTimer(this);
    m_reconnectTimer->setInterval(5000); // 5秒重连一次
    connect(m_reconnectTimer, &QTimer::timeout, this, &TaskServerNode::reconnectTimerTimeout);
    
    // 尝试连接机器人
    connectToRobot();
    
    // 启动重连定时器
    m_reconnectTimer->start();
    LOG_INFO("重连定时器已启动，间隔: 5秒");
}

/**
 * @brief 尝试连接机器人
 */
void TaskServerNode::connectToRobot()
{
    LOG_DEBUG("尝试连接到机器人 - IP: {}, 导航端口: {}, 状态端口: {}", 
                  m_seerIP.toStdString(), m_naviPort, m_confPort);
    
    // 检查并连接导航套接字（仅当未连接时）
    if (!isNaviConnected()) {
        LOG_INFO("尝试重新连接导航端口");
        int naviResult = m_naviSocket->connectHost(m_seerIP, m_naviPort);
        if (naviResult == 0) {
            LOG_INFO("导航端口连接请求已发送");
        } else {
            LOG_WARN("导航端口连接请求发送失败或关闭了现有连接");
        }
    } else {
        LOG_DEBUG("导航端口已连接，无需重新连接");
    }
    
    // 检查并连接状态套接字（仅当未连接时）
    if (!isConfConnected()) {
        LOG_INFO("尝试重新连接状态端口");
        int statResult = m_confSocket->connectHost(m_seerIP, m_confPort);
        if (statResult == 0) {
            LOG_INFO("状态端口连接请求已发送");
        } else {
            LOG_WARN("状态端口连接请求发送失败或关闭了现有连接");
        }
    } else {
        LOG_DEBUG("状态端口已连接，无需重新连接");
    }
    
    // 检查并连接其他任务套接字（仅当未连接时）
    if (!isOtherConnected()) {
        LOG_DEBUG("尝试连接到其他任务 - IP: {}, 端口: {}", 
                      m_seerIP.toStdString(), m_otherPort);
        int otherResult = m_otherSocket->connectHost(m_seerIP, m_otherPort);
        if (otherResult == 0) {
            LOG_INFO("其他任务端口连接请求已发送");
        } else {
            LOG_WARN("其他任务端口连接请求发送失败或关闭了现有连接");
        }
    } else {
        LOG_DEBUG("其他任务端口已连接，无需重新连接");
    }
    
    // 检查并连接YOLO任务套接字（仅当未连接时）
    if (!isYoloConnected()) {
        LOG_DEBUG("尝试连接到YOLO任务 - m_yoloIP: {}, 端口: {}", 
                      m_yoloIP.toStdString(), m_yoloPort);
        int yoloResult = m_yoloSocket->connectHost(m_yoloIP, m_yoloPort);
        if (yoloResult == 0) {
            LOG_INFO("YOLO任务端口连接请求已发送");
        } else {
            LOG_WARN("YOLO任务端口连接请求发送失败或关闭了现有连接");
        }
    } else {
        LOG_DEBUG("YOLO任务端口已连接，无需重新连接");
    }
}

/**
* @brief 状态端口状态改变处理函数
*/
void TaskServerNode::handleSocketStateChanged(QAbstractSocket::SocketState state)
{
    QTcpSocket* socket = qobject_cast<QTcpSocket*>(sender());
    QString socketName = (socket == m_naviSocket->tcpSocket()) ? "导航" : 
                        (socket == m_confSocket->tcpSocket()) ? "状态" : 
                        (socket == m_otherSocket->tcpSocket()) ? "其他" : "YOLO任务";
    
    switch (state) {
        case QAbstractSocket::UnconnectedState:
            LOG_DEBUG("{}套接字状态: 未连接", socketName.toStdString());
            break;
        case QAbstractSocket::HostLookupState:
            LOG_DEBUG("{}套接字状态: 正在查找主机", socketName.toStdString());
            break;
        case QAbstractSocket::ConnectingState:
            LOG_DEBUG("{}套接字状态: 正在连接", socketName.toStdString());
            break;
        case QAbstractSocket::ConnectedState:
            LOG_INFO("{}套接字状态: 已连接", socketName.toStdString());
            
            // 更新对应套接字的连接状态
            if (socket == m_naviSocket->tcpSocket()) {
                m_naviConnected = true;
                m_naviReconnectAttempts = 0; // 重置重连计数
            } else if (socket == m_confSocket->tcpSocket()) {
                m_confConnected = true;
                m_confReconnectAttempts = 0;
            } else if (socket == m_otherSocket->tcpSocket()) {
                m_otherConnected = true;
                m_otherReconnectAttempts = 0;
            } else if (socket == m_yoloSocket->tcpSocket()) {
                m_yoloConnected = true;
                m_yoloReconnectAttempts = 0;
            }
            
            // 检查是否所有连接都已建立
            if (isAllConnected()) {
                LOG_INFO("所有机器人连接已建立");
                m_wasConnected = true;
                resetReconnectAttempts(); // 重置总重连计数
            }
            break;
        case QAbstractSocket::BoundState:
            LOG_DEBUG("{}套接字状态: 已绑定", socketName.toStdString());
            break;
        case QAbstractSocket::ListeningState:
            LOG_DEBUG("{}套接字状态: 监听中", socketName.toStdString());
            break;
        case QAbstractSocket::ClosingState:
            LOG_DEBUG("{}套接字状态: 正在关闭", socketName.toStdString());
            break;
    }
    
    LOG_DEBUG("当前连接状态 - 导航: {}, 状态: {}, 其他: {}, YOLO: {}", 
                   m_naviSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState ? "已连接" : "未连接",
                   m_confSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState ? "已连接" : "未连接",
                   m_otherSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState ? "已连接" : "未连接",
                   m_yoloSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState ? "已连接" : "未连接");
}

/**
* @brief 套接字错误处理
*/
void TaskServerNode::handleSocketError(QAbstractSocket::SocketError error)
{
    QTcpSocket* socket = qobject_cast<QTcpSocket*>(sender());
    QString socketName = (socket == m_naviSocket->tcpSocket()) ? "导航" : 
                        (socket == m_confSocket->tcpSocket()) ? "状态" : 
                        (socket == m_otherSocket->tcpSocket()) ? "其他" : "YOLO任务";
    
    LOG_ERROR("{}套接字错误: {}", socketName.toStdString(), static_cast<int>(error));
    
    // 更新对应套接字的连接状态
    if (socket == m_naviSocket->tcpSocket()) {
        m_naviConnected = false;
    } else if (socket == m_confSocket->tcpSocket()) {
        m_confConnected = false;
    } else if (socket == m_otherSocket->tcpSocket()) {
        m_otherConnected = false;
    } else if (socket == m_yoloSocket->tcpSocket()) {
        m_yoloConnected = false;
    }
    
    switch (error) {
        case QAbstractSocket::ConnectionRefusedError:
            LOG_ERROR("{}连接被拒绝", socketName.toStdString());
            break;
        case QAbstractSocket::RemoteHostClosedError:
            LOG_WARN("{}远程主机关闭连接", socketName.toStdString());
            break;
        case QAbstractSocket::HostNotFoundError:
            LOG_ERROR("{}找不到主机", socketName.toStdString());
            break;
        case QAbstractSocket::SocketAccessError:
            LOG_ERROR("{}套接字访问错误", socketName.toStdString());
            break;
        case QAbstractSocket::SocketResourceError:
            LOG_ERROR("{}套接字资源错误", socketName.toStdString());
            break;
        case QAbstractSocket::SocketTimeoutError:
            LOG_ERROR("{}套接字超时", socketName.toStdString());
            break;
        case QAbstractSocket::DatagramTooLargeError:
            LOG_ERROR("{}数据报太大", socketName.toStdString());
            break;
        case QAbstractSocket::NetworkError:
            LOG_ERROR("{}网络错误", socketName.toStdString());
            break;
        case QAbstractSocket::AddressInUseError:
            LOG_ERROR("{}地址已被使用", socketName.toStdString());
            break;
        case QAbstractSocket::SocketAddressNotAvailableError:
            LOG_ERROR("{}套接字地址不可用", socketName.toStdString());
            break;
        case QAbstractSocket::UnsupportedSocketOperationError:
            LOG_ERROR("{}不支持的套接字操作", socketName.toStdString());
            break;
        case QAbstractSocket::UnfinishedSocketOperationError:
            LOG_ERROR("{}未完成的套接字操作", socketName.toStdString());
            break;
        case QAbstractSocket::ProxyAuthenticationRequiredError:
            LOG_ERROR("{}代理需要认证", socketName.toStdString());
            break;
        case QAbstractSocket::SslHandshakeFailedError:
            LOG_ERROR("{}SSL握手失败", socketName.toStdString());
            break;
        case QAbstractSocket::ProxyConnectionRefusedError:
            LOG_ERROR("{}代理连接被拒绝", socketName.toStdString());
            break;
        case QAbstractSocket::ProxyConnectionClosedError:
            LOG_ERROR("{}代理连接已关闭", socketName.toStdString());
            break;
        case QAbstractSocket::ProxyConnectionTimeoutError:
            LOG_ERROR("{}代理连接超时", socketName.toStdString());
            break;
        case QAbstractSocket::ProxyNotFoundError:
            LOG_ERROR("{}找不到代理", socketName.toStdString());
            break;
        case QAbstractSocket::ProxyProtocolError:
            LOG_ERROR("{}代理协议错误", socketName.toStdString());
            break;
        case QAbstractSocket::OperationError:
            LOG_ERROR("{}操作错误", socketName.toStdString());
            break;
        case QAbstractSocket::SslInternalError:
            LOG_ERROR("{}SSL内部错误", socketName.toStdString());
            break;
        case QAbstractSocket::SslInvalidUserDataError:
            LOG_ERROR("{}SSL无效用户数据", socketName.toStdString());
            break;
        case QAbstractSocket::TemporaryError:
            LOG_ERROR("{}临时错误", socketName.toStdString());
            break;
        default:
            LOG_ERROR("{}未知错误", socketName.toStdString());
            break;
    }
    
    // 使用静态变量来避免在短时间内多次增加重连计数
    static QDateTime lastReconnectTime;
    static QMutex reconnectMutex;
    QDateTime currentTime = QDateTime::currentDateTime();
    
    QMutexLocker locker(&reconnectMutex);
    // 如果距离上次重连时间超过1秒，或者这是第一次重连，则增加重连计数
    if (lastReconnectTime.isNull() || lastReconnectTime.secsTo(currentTime) >= 1) {
        LOG_INFO("检测到套接字错误，计划重新连接");
        lastReconnectTime = currentTime;
        scheduleReconnect(); // 使用改进的重连机制
    } else {
        LOG_INFO("检测到套接字错误，但距离上次重连时间不足1秒，不增加重连计数");
        // 仍然尝试连接，但不增加计数
        QTimer::singleShot(1000, this, &TaskServerNode::connectToRobot);
    }
}

/**
 * @brief 重连定时器超时处理函数
 * 
 * 当重连定时器超时时调用此函数，用于检查与导航服务器和状态服务器的连接状态。
 * 如果检测到任何一个连接断开，将触发重新连接流程。
 * 
 * 该函数会检查以下连接：
 * - 导航套接字连接 (m_naviSocket)
 * - 状态套接字连接 (m_confSocket)
 * - 其他任务套接字连接 (m_otherSocket)
 * - YOLO任务套接字连接 (m_yoloSocket)
 * 
 * 如果任一连接断开，则调用 connectToRobot() 进行重连。
 */
void TaskServerNode::reconnectTimerTimeout()
{
    LOG_DEBUG("重连定时器超时，检查连接状态");
    updateConnectionStatus();
    
    bool needReconnect = false;
    
    // 检查导航连接
    if (!isNaviConnected()) {
        LOG_DEBUG("导航连接断开，需要重连");
        needReconnect = true;
    }
    
    // 检查状态连接
    if (!isConfConnected()) {
        LOG_DEBUG("状态连接断开，需要重连");
        needReconnect = true;
    }
    
    // 检查其他任务连接
    if (!isOtherConnected()) {
        LOG_DEBUG("其他任务连接断开，需要重连");
        needReconnect = true;
    }
    
    // 检查YOLO任务连接
    if (!isYoloConnected()) {
        LOG_DEBUG("YOLO任务连接断开，需要重连");
        needReconnect = true;
    }
    
    // 如果需要重连，则调用连接函数
    if (needReconnect) {
        LOG_INFO("检测到连接断开，尝试重新连接");
        connectToRobot();
    } else {
        LOG_DEBUG("所有连接正常，无需重连");
        // 如果连接正常，确保重连计数器被重置
        if (m_naviReconnectAttempts > 0 || 
            m_confReconnectAttempts > 0 || m_otherReconnectAttempts > 0 || 
            m_yoloReconnectAttempts > 0) {
            LOG_DEBUG("连接正常，重置重连计数器");
            resetReconnectAttempts();
        }
    }
}

/**
 * @brief 检查所有套接字连接状态
 * 
 * 该函数用于检查与机器人控制器的所有TCP连接是否都处于连接状态，
 * 包括导航、配置、其他任务和YOLO图像处理四个套接字连接。
 * 同时会更新对应的成员变量以反映最新的连接状态。
 * 
 * @return bool 如果所有套接字都处于连接状态返回true，否则返回false
 */
bool TaskServerNode::isAllConnected() const
{
    // 直接调用各个套接字的连接检查函数，更加简洁明了
    bool naviConnected = isNaviConnected();
    bool statConnected = isConfConnected();
    bool otherConnected = isOtherConnected();
    bool yoloConnected = isYoloConnected();
    
    // 更新连接状态变量
    const_cast<TaskServerNode*>(this)->m_naviConnected = naviConnected;
    const_cast<TaskServerNode*>(this)->m_confConnected = statConnected;
    const_cast<TaskServerNode*>(this)->m_otherConnected = otherConnected;
    const_cast<TaskServerNode*>(this)->m_yoloConnected = yoloConnected;
    
    return naviConnected && statConnected && otherConnected && yoloConnected;
}

/**
 * @brief 检查导航套接字连接状态
 */
bool TaskServerNode::isNaviConnected() const
{
    if (!m_naviSocket || !m_naviSocket->tcpSocket()) {
        return false;
    }
    
    bool connected = (m_naviSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState);
    const_cast<TaskServerNode*>(this)->m_naviConnected = connected;
    return connected;
}

/**
 * @brief 检查配置套接字连接状态（抢占控制权）
 */
bool TaskServerNode::isConfConnected() const
{
    if (!m_confSocket || !m_confSocket->tcpSocket()) {
        return false;
    }
    
    bool connected = (m_confSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState);
    const_cast<TaskServerNode*>(this)->m_confConnected = connected;
    return connected;
}

/**
 * @brief 检查其他任务套接字连接状态（软急停/设置DO）
 */
bool TaskServerNode::isOtherConnected() const
{
    if (!m_otherSocket || !m_otherSocket->tcpSocket()) {
        return false;
    }
    
    bool connected = (m_otherSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState);
    const_cast<TaskServerNode*>(this)->m_otherConnected = connected;
    return connected;
}

/**
 * @brief 检查YOLO任务套接字连接状态
 */
bool TaskServerNode::isYoloConnected() const
{
    if (!m_yoloSocket || !m_yoloSocket->tcpSocket()) {
        return false;
    }
    
    bool connected = (m_yoloSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState);
    const_cast<TaskServerNode*>(this)->m_yoloConnected = connected;
    return connected;
}

/**
 * @brief 检查关键套接字连接状态(导航和配置)
 */
bool TaskServerNode::areAllCriticalConnected() const
{
    return isNaviConnected() && isConfConnected();
}

/**
 * @brief 处理Action目标请求
 * 
 * 当客户端发送一个任务执行请求时，此函数会被调用。函数会验证请求的有效性，
 * 包括检查机器人连接状态、动作类型和命令ID是否支持。
 * 
 * @param uuid 目标UUID，用于唯一标识该目标请求
 * @param goal 指向目标信息的共享指针，包含任务详细信息
 * @return rclcpp_action::GoalResponse 目标响应结果，可能为接受或拒绝
 */
rclcpp_action::GoalResponse TaskServerNode::handle_goal(
    const rclcpp_action::GoalUUID &/*uuid*/,
    std::shared_ptr<const TaskExecution::Goal> goal)
{
    LOG_INFO("Received goal request for task: {}, type: {}, action: {}, cmd_id: {} start_location: {}, end_location: {} lobe_data: {}", 
                goal->task_id, goal->task_type, goal->action, goal->cmd_id,goal->task_start_location,goal->task_end_location,goal->lobe_data);

    if (!areAllCriticalConnected()) {
        LOG_WARN("机器人关键连接未建立，拒绝目标请求");
        return rclcpp_action::GoalResponse::REJECT;
    }

    // 只接受 "start/resume/pause/cancel" 作为初始动作
    if (goal->action != "start" && goal->action != "resume" && goal->action != "pause" && goal->action != "cancel")  {
        LOG_WARN("Only 'start' or 'resume' or 'pause' or 'cancel' allowed for new goals.");
        return rclcpp_action::GoalResponse::REJECT;
    }

    // 检查cmd_id是否支持
    if (goal->cmd_id != 3001 && goal->cmd_id != 3002 && goal->cmd_id != 3003 && goal->cmd_id != 3066) {
        LOG_WARN("Unsupported cmd_id: {}. Only 3001/3002/3003/3066 (navigation) are supported.",goal->cmd_id);
        return rclcpp_action::GoalResponse::REJECT;
    }

    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

/**
 * @brief 处理已接受的目标请求
 * 
 * 当Action服务器接受一个目标后，此函数会被调用。函数将创建一个新线程来执行任务，
 * 以避免阻塞主线程，确保系统响应性。
 * 
 * @param goal_handle 已接受的目标句柄，用于执行任务操作
 */
void TaskServerNode::handle_accepted(const std::shared_ptr<GoalHandle> goal_handle)
{
    LOG_DEBUG("Goal accepted, starting execution");
    // 添加错误处理以防止段错误
    try {
        QMetaObject::invokeMethod(this, [=]() {
            execute_task(goal_handle);
        }, Qt::QueuedConnection);
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in handle_accepted: {}", e.what());
    } catch (...) {
        LOG_ERROR("Unknown exception in handle_accepted");
    }
}

/**
 * @brief 执行任务的主要函数
 * 
 * 根据目标中的cmd_id执行不同类型的任务。支持的任务类型包括:
 * - 3001: 暂停任务
 * - 3002: 恢复任务
 * - 3003: 取消任务
 * - 3066: 路径导航任务
 * 
 * 对于不支持的cmd_id，函数会记录错误并中止任务执行。
 * 
 * @param goal_handle 任务目标句柄，包含任务目标信息和用于发送反馈与结果的接口
 */
void TaskServerNode::execute_task(const std::shared_ptr<GoalHandle> goal_handle)
{
    const auto goal = goal_handle->get_goal();
    // 检查goal是否有效
    if (!goal) {
        LOG_ERROR("无效的目标");
        return;
    }
    
    auto feedback = std::make_shared<TaskExecution::Feedback>();
    auto result = std::make_shared<TaskExecution::Result>();

    feedback->task_id = goal->task_id;
    feedback->current_location = "robot_current_position";

    // 记录开始时间
    auto start_time = this->get_clock()->now();
    
    LOG_INFO("Executing task: {}, cmd_id: {}", 
                goal->task_id, goal->cmd_id);

    // 如果是start操作，记录开始时间并启动监控
    if (goal->action == "start") {
        // 创建任务目录
        createTaskDirectory(goal->task_id);
        TaskContext context;
        context.start_time = start_time;
        context.task_id = goal->task_id;
        context.goal = goal; // 保存任务目标
        context.goal_handle = goal_handle;
        m_taskContexts[goal->task_id] = context;
        
        // 启动导航状态监控
        startNavigationMonitoring(goal->task_id);
    }

    // 直接使用metadata作为JSON数据发送
    QByteArray jsonData = goal->metadata.c_str();
    
    // 确保拥有控制权（在执行任何命令前检查一次）
    if (!hasControlAuthority) {
        LOG_INFO("没有控制权, 抢占控制权... jsonData: {}",jsonData.toStdString());
        
        // 保存当前任务上下文
        m_currentTaskContext = std::make_unique<CurrentTaskContext>();
        m_currentTaskContext->goal = goal;
        m_currentTaskContext->goal_handle = goal_handle;
        m_currentTaskContext->start_time = start_time;
        m_currentTaskContext->json_data = jsonData;
        
        // 发送控制权获取请求，不阻塞
        acquireControlAuthority(QString::fromStdString(m_acquirerControlName), CMD_GET_CONTROL_AUTHORITY);
        
        return; // 不阻塞，直接返回，等待异步回调
    }
    
    // 如果已经有控制权，直接执行任务
    // 确保在执行任务命令前设置当前任务上下文
    m_currentTaskContext = std::make_unique<CurrentTaskContext>();
    m_currentTaskContext->goal = goal;
    m_currentTaskContext->goal_handle = goal_handle;
    m_currentTaskContext->start_time = start_time;
    m_currentTaskContext->json_data = jsonData;
    
    executeTaskCommand(goal, jsonData, goal_handle, start_time);
}


/**
 * @brief 处理取消请求
 * 
 * 当客户端请求取消正在执行的任务时，此函数会被调用。
 * 函数会记录取消请求并返回接受取消的响应。
 * 
 * @param goal_handle 指向目标句柄的共享指针，可用于获取目标信息
 * @return rclcpp_action::CancelResponse 取消响应结果，表示接受或拒绝取消请求
 */
rclcpp_action::CancelResponse TaskServerNode::handle_cancel(
    const std::shared_ptr<GoalHandle> goal_handle)
{
    LOG_INFO("Received request to cancel task: {}", 
                goal_handle->get_goal()->task_id);
    return rclcpp_action::CancelResponse::ACCEPT;
}

// 健康检查定时器槽函数
void TaskServerNode::healthCheckTimeout()
{
    //LOG_INFO("执行健康检查");
    healthCheck();
}

// 健康检查函数
void TaskServerNode::healthCheck()
{
    // 检查当前连接状态
    updateConnectionStatus();
    
    // 如果连接断开，触发重连
    if (!isAllConnected()) {
        LOG_DEBUG("健康检查发现连接断开，计划重连");
        scheduleReconnect();
    } else {
        LOG_DEBUG("健康检查: 连接正常");
        // 检查服务客户端是否需要重新创建
        if (!m_robotStatusClient->service_is_ready()) {
            LOG_WARN("机器人状态服务不可用，尝试重新创建服务客户端");
            // 重新创建服务客户端
            std::string node_namespace = this->get_namespace();
            std::string service_name;

            if (node_namespace == "/") {
                service_name = "get_robot_status";
            } else {
                service_name = node_namespace + "/get_robot_status";
            }

            m_robotStatusClient = this->create_client<rms_pkg::srv::RobotStatus>(service_name);
            LOG_INFO("重新创建服务客户端: {}", service_name);
        }
        // 可以在这里发送心跳包等健康检查操作
    }
}

// 更新连接状态
void TaskServerNode::updateConnectionStatus()
{
    bool naviConnected = isNaviConnected();
    bool statConnected = isConfConnected();
    bool otherConnected = isOtherConnected();
    bool yoloConnected = isYoloConnected();
    
    if (naviConnected && statConnected && otherConnected && yoloConnected && !m_wasConnected) {
        LOG_INFO("所有连接已恢复");
        m_wasConnected = true;
        resetReconnectAttempts(); // 重置重连次数
    } else if ((!naviConnected || !statConnected || !otherConnected || !yoloConnected) && m_wasConnected) {
        LOG_WARN("连接断开，开始重连流程");
        m_wasConnected = false;
    }
}

/**
 * @brief 使用指数退避算法调度机器人重连
 * 
 * 该函数实现了一个带有抖动的指数退避重连机制，防止在网络不稳定时频繁重连。
 * 重连延迟时间随着重连尝试次数增加而指数增长，并添加随机抖动避免同步重试。
 * 当重连次数达到最大限制时，将使用固定间隔继续尝试重连。
 * 
 * @note 重连延迟范围在1秒到60秒之间，包含±20%的随机抖动
 */
void TaskServerNode::scheduleReconnect()
{
    // 检查各个套接字的连接状态并分别处理重连逻辑
    
    // 处理导航套接字重连
    if (m_naviSocket && m_naviSocket->tcpSocket()) {
        bool naviConnected = isNaviConnected();
        if (!naviConnected) {
            int delay;
            if (m_naviReconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
                int baseDelay = 1000; // 基础延迟1秒
                int maxDelay = 60000; // 最大延迟60秒

                int exponentialFactor = 1;
                if (m_naviReconnectAttempts < 31) {
                    exponentialFactor = (1 << m_naviReconnectAttempts);
                } else {
                    exponentialFactor = maxDelay / baseDelay;
                }

                delay = std::min(baseDelay * exponentialFactor, maxDelay);

                // 添加抖动 (±20%)
                int jitter = (rand() % (delay / 5)) - (delay / 10);
                delay += jitter;
                delay = std::max(1000, delay); // 最小1秒

                m_naviReconnectAttempts++;
                LOG_INFO("计划在 {} ms 后进行导航套接字第 {} 次重连", delay, m_naviReconnectAttempts);

                QTimer::singleShot(delay, this, [this]() {
                    if (!isNaviConnected()) {
                        LOG_INFO("尝试重新连接导航端口");
                        int result = m_naviSocket->connectHost(m_seerIP, m_naviPort);
                        if (result == 0) {
                            LOG_INFO("导航端口连接请求已发送");
                        } else {
                            LOG_WARN("导航端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            } else {
                LOG_INFO("导航套接字已达到最大重连次数({})，将继续以 {} ms 间隔尝试重连", MAX_RECONNECT_ATTEMPTS, 60000);
                QTimer::singleShot(60000, this, [this]() {
                    if (!isNaviConnected()) {
                        LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
                        int result = m_naviSocket->connectHost(m_seerIP, m_naviPort);
                        if (result == 0) {
                            LOG_INFO("导航端口连接请求已发送");
                        } else {
                            LOG_WARN("导航端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            }
        }
    }
    
    // 处理状态套接字重连
    if (m_confSocket && m_confSocket->tcpSocket()) {
        bool statConnected = isConfConnected();
        if (!statConnected) {
            int delay;
            if (m_confReconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
                int baseDelay = 1000; // 基础延迟1秒
                int maxDelay = 60000; // 最大延迟60秒
                
                int exponentialFactor = 1;
                if (m_confReconnectAttempts < 31) {
                    exponentialFactor = (1 << m_confReconnectAttempts);
                } else {
                    exponentialFactor = maxDelay / baseDelay;
                }
                
                delay = std::min(baseDelay * exponentialFactor, maxDelay);

                // 添加抖动 (±20%)
                int jitter = (rand() % (delay / 5)) - (delay / 10);
                delay += jitter;
                delay = std::max(1000, delay); // 最小1秒

                m_confReconnectAttempts++;
                LOG_INFO("计划在 {} ms 后进行状态套接字第 {} 次重连", delay, m_confReconnectAttempts);

                QTimer::singleShot(delay, this, [this]() {
                    if (!isConfConnected()) {
                        LOG_INFO("尝试重新连接状态端口");
                        int result = m_confSocket->connectHost(m_seerIP, m_confPort);
                        if (result == 0) {
                            LOG_INFO("状态端口连接请求已发送");
                        } else {
                            LOG_WARN("状态端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            } else {
                LOG_INFO("状态套接字已达到最大重连次数({})，将继续以 {} ms 间隔尝试重连", MAX_RECONNECT_ATTEMPTS, 60000);
                QTimer::singleShot(60000, this, [this]() {
                    if (!isConfConnected()) {
                        LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
                        int result = m_confSocket->connectHost(m_seerIP, m_confPort);
                        if (result == 0) {
                            LOG_INFO("状态端口连接请求已发送");
                        } else {
                            LOG_WARN("状态端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            }
        }
    }
    
    // 处理其他任务套接字重连
    if (m_otherSocket && m_otherSocket->tcpSocket()) {
        bool otherConnected = isOtherConnected();
        if (!otherConnected) {
            int delay;
            if (m_otherReconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
                int baseDelay = 1000; // 基础延迟1秒
                int maxDelay = 60000; // 最大延迟60秒
                
                int exponentialFactor = 1;
                if (m_otherReconnectAttempts < 31) {
                    exponentialFactor = (1 << m_otherReconnectAttempts);
                } else {
                    exponentialFactor = maxDelay / baseDelay;
                }
                
                delay = std::min(baseDelay * exponentialFactor, maxDelay);

                // 添加抖动 (±20%)
                int jitter = (rand() % (delay / 5)) - (delay / 10);
                delay += jitter;
                delay = std::max(1000, delay); // 最小1秒

                m_otherReconnectAttempts++;
                LOG_INFO("计划在 {} ms 后进行其他任务套接字第 {} 次重连", delay, m_otherReconnectAttempts);

                QTimer::singleShot(delay, this, [this]() {
                    if (!isOtherConnected()) {
                        LOG_INFO("尝试重新连接其他任务端口");
                        int result = m_otherSocket->connectHost(m_seerIP, m_otherPort);
                        if (result == 0) {
                            LOG_INFO("其他任务端口连接请求已发送");
                        } else {
                            LOG_WARN("其他任务端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            } else {
                LOG_INFO("其他任务套接字已达到最大重连次数({})，将继续以 {} ms 间隔尝试重连", MAX_RECONNECT_ATTEMPTS, 60000);
                QTimer::singleShot(60000, this, [this]() {
                    if (!isOtherConnected()) {
                        LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
                        int result = m_otherSocket->connectHost(m_seerIP, m_otherPort);
                        if (result == 0) {
                            LOG_INFO("其他任务端口连接请求已发送");
                        } else {
                            LOG_WARN("其他任务端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            }
        }
    }
    
    // 处理YOLO任务套接字重连
    if (m_yoloSocket && m_yoloSocket->tcpSocket()) {
        bool yoloConnected = isYoloConnected();
        if (!yoloConnected) {
            int delay;
            if (m_yoloReconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
                int baseDelay = 1000; // 基础延迟1秒
                int maxDelay = 60000; // 最大延迟60秒
                
                int exponentialFactor = 1;
                if (m_yoloReconnectAttempts < 31) {
                    exponentialFactor = (1 << m_yoloReconnectAttempts);
                } else {
                    exponentialFactor = maxDelay / baseDelay;
                }
                
                delay = std::min(baseDelay * exponentialFactor, maxDelay);

                // 添加抖动 (±20%)
                int jitter = (rand() % (delay / 5)) - (delay / 10);
                delay += jitter;
                delay = std::max(1000, delay); // 最小1秒

                m_yoloReconnectAttempts++;
                LOG_INFO("计划在 {} ms 后进行YOLO任务套接字第 {} 次重连", delay, m_yoloReconnectAttempts);

                QTimer::singleShot(delay, this, [this]() {
                    if (!isYoloConnected()) {
                        LOG_INFO("尝试重新连接YOLO任务端口");
                        int result = m_yoloSocket->connectHost(m_yoloIP, m_yoloPort);
                        if (result == 0) {
                            LOG_INFO("YOLO任务端口连接请求已发送");
                        } else {
                            LOG_WARN("YOLO任务端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            } else {
                LOG_INFO("YOLO任务套接字已达到最大重连次数({})，将继续以 {} ms 间隔尝试重连", MAX_RECONNECT_ATTEMPTS, 60000);
                QTimer::singleShot(60000, this, [this]() {
                    if (!isYoloConnected()) {
                        LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
                        int result = m_yoloSocket->connectHost(m_yoloIP, m_yoloPort);
                        if (result == 0) {
                            LOG_INFO("YOLO任务端口连接请求已发送");
                        } else {
                            LOG_WARN("YOLO任务端口连接请求发送失败或关闭了现有连接");
                        }
                    }
                });
            }
        }
    }
    
    // 更新连接状态
    m_naviConnected = isNaviConnected();
    m_confConnected = isConfConnected();
    m_otherConnected = isOtherConnected();
    m_yoloConnected = isYoloConnected();
}

// 重置重连尝试次数
void TaskServerNode::resetReconnectAttempts()
{
    m_naviReconnectAttempts = 0;
    m_confReconnectAttempts = 0;
    m_otherReconnectAttempts = 0;
    m_yoloReconnectAttempts = 0;
    LOG_DEBUG("所有套接字的重连尝试次数已重置");
}

/**
 * @brief 启动指定任务的导航状态监控
 * 
 * 该函数为指定的任务创建一个定时器，用于定期查询机器人的导航状态。
 * 定时器每秒触发一次，调用queryNavigationStatus函数来获取最新的导航状态。
 * 
 * @param task_id 任务ID，用于标识需要监控的具体任务
 */
void TaskServerNode::startNavigationMonitoring(const std::string& task_id)
{
    LOG_INFO("Starting navigation monitoring for task: {}", task_id);
    
    // 创建定时器以定期查询导航状态
    auto timer = std::make_shared<QTimer>(this);
    timer->setInterval(1000); // 每秒查询一次
    
    connect(timer.get(), &QTimer::timeout, this, [this, task_id, timer]() {
        queryNavigationStatus(task_id, timer);
    });
    
    m_navigationTimers[task_id] = timer;
    timer->start();
}

/**
 * @brief 查询指定任务的机器人导航状态
 * 
 * 该函数通过异步调用机器人状态服务来获取当前任务的导航状态信息。
 * 主要功能包括检查任务上下文是否存在、验证服务可用性以及发送异步请求。
 * 
 * @param task_id 任务ID，用于标识需要查询状态的具体任务
 * @param timer 定时器共享指针，用于控制查询过程的时间管理
 */
void TaskServerNode::queryNavigationStatus(const std::string& task_id, std::shared_ptr<QTimer> timer)
{
    // 检查任务上下文是否存在
    if (m_taskContexts.find(task_id) == m_taskContexts.end()) {
        LOG_WARN("Task context not found for task: {}", task_id);
        timer->stop();
        return;
    }
    
    auto context = m_taskContexts[task_id];

    // 检查服务是否可用
    if (!m_robotStatusClient->service_is_ready()) {
        LOG_WARN("Robot status service not available");
        // 检查连接状态，如果连接已恢复但服务仍不可用，则尝试重新创建服务客户端
        if (isAllConnected()) {
            LOG_WARN("连接已恢复但服务仍不可用，尝试重新创建服务客户端");
            std::string node_namespace = this->get_namespace();
            std::string service_name;

            if (node_namespace == "/") {
                service_name = "get_robot_status";
            } else {
                service_name = node_namespace + "/get_robot_status";
            }

            m_robotStatusClient = this->create_client<rms_pkg::srv::RobotStatus>(service_name);
            LOG_INFO("重新创建服务客户端: {}", service_name);
        }
        return;
    }
    
    // 使用异步方式发送请求，类似于 heartbeat_publisher 中的成功实现
    auto request = std::make_shared<rms_pkg::srv::RobotStatus::Request>();
    // 设置查询类型为导航和控制
    request->query_type = rms_pkg::srv::RobotStatus::Request::QUERY_NAVIGATION_AND_CONTROL;
    
    // 保存 task_id 以便在回调中使用
    auto task_id_copy = task_id;
    auto timer_copy = timer;

    m_robotStatusClient->async_send_request(
        request,
        [this, task_id_copy, timer_copy](rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future) {
            this->handleRobotStatusResponse(task_id_copy, timer_copy, future);
        }
    );

    //LOG_DEBUG("Robot status request sent asynchronously for task: {}", task_id);
}

/**
 * @brief 处理机器人状态响应
 * 
 * 该函数处理从机器人状态服务返回的异步响应，解析导航状态并根据任务状态决定是否结束任务。
 * 当任务完成、失败或取消时，停止监控定时器，导出传感器数据并保存任务结果到数据库。
 * 
 * @param task_id 任务ID，用于标识当前处理的任务
 * @param timer 定时器共享指针，当任务完成时用于停止状态查询
 * @param future 服务调用结果，包含机器人的导航状态和控制权信息
 */
void TaskServerNode::handleRobotStatusResponse(const std::string& task_id, 
                                              std::shared_ptr<QTimer> timer,
                                              rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future)
{    
    try {
        // 检查任务上下文是否存在
        if (m_taskContexts.find(task_id) == m_taskContexts.end()) {
            LOG_WARN("Task context not found for task: {}", task_id);
            timer->stop();
            return;
        }
        
        auto context = m_taskContexts[task_id];
        
        // 获取响应
        auto result = future.get();
        //LOG_DEBUG("Response success: {}, message: {}", result->success, result->message);
        
        if (!result->success) {
            LOG_WARN("Failed to get robot status: {}", result->message);
            return;
        }

        // 解析导航状态
        QString navState = QString::fromStdString(result->navigation_state);

        // 是否拥有控制权判断
        hasControlAuthority = (result->control_owner == m_acquirerControlName);
        
        int taskStatus = getTaskStatus(navState);
        
        //LOG_DEBUG("Navigation status for task {}: {} ({}) control_owner:{} hasControlAuthority:{}", task_id, navState.toStdString(), taskStatus, result->control_owner, hasControlAuthority);
        
        // 检查是否任务已完成、失败或取消(0 = NONE, 1 = WAITING(目前不可能出现该状态), 2 = RUNNING, 3 = SUSPENDED, 4 = COMPLETED, 5 = FAILED, 6 = CANCELED)
        if (taskStatus == 4 || taskStatus == 5 || taskStatus == 6) {
            // 停止定时器
            timer->stop();

            // 记录结束时间
            context.end_time = this->get_clock()->now();
            m_taskContexts[task_id] = context;

            // 处理特定任务类型的清理工作
            handleTaskTypeSpecificCleanup(context.goal);

            // 导出传感器数据
            exportSensorData(task_id, context.start_time, context.end_time);

            // 保存任务结果到数据库
            saveTaskResultToDatabase(task_id, taskStatus, context.start_time, context.end_time);

            LOG_INFO("Task {} completed with status: {}", task_id, navState.toStdString());

             // 任务已经结束，直接返回，不再处理后续逻辑
            return;
        }
        
        // 向YOLO子任务发送位置更新信息（如果任务类型是巡检任务）
        if (context.goal && context.goal->task_type == "inspection") {
            QString currentTarget = QString::fromStdString(result->current_target);     //当前目标站点
            QString lastStation = QString::fromStdString(result->last_station);         //上一个站点
            //LOG_INFO("lastStation: {} currentTarget: {}", lastStation.toStdString(), currentTarget.toStdString());
            sendLocationUpdateToYolo(task_id, lastStation, currentTarget);
        }else if (context.goal && context.goal->task_type == "spraying") {
            // 处理喷药任务逻辑
            handleSprayingTask(task_id, result, context);
        }
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in handleRobotStatusResponse: {}", e.what());
    } catch (...) {
        LOG_ERROR("Unknown exception in handleRobotStatusResponse");
    }
}

/**
 * @brief 发送任务开始数据给YOLO子任务
 * 
 * @param task_id 任务ID
 * @param goal 任务目标
 */
void TaskServerNode::sendTaskStartToYolo(const std::string& task_id, 
                                       const std::shared_ptr<const TaskExecution::Goal> goal)
{
    LOG_INFO("准备发送START任务数据给YOLO子任务，任务ID: {}", task_id);
    
    // 检查连接状态
    if (!isYoloConnected()) {
        LOG_WARN("YOLO socket未连接，无法发送任务开始数据");
        return;
    }
    
    // 构造JSON数据
    QJsonObject jsonRoot;
    
    // 解析lobe_data中的lobe_definition
    QJsonParseError parseError;
    QJsonDocument lobeDoc = QJsonDocument::fromJson(goal->lobe_data.c_str(), &parseError);
    if (parseError.error == QJsonParseError::NoError && lobeDoc.isObject()) {
        QJsonObject lobeObj = lobeDoc.object();
        if (lobeObj.contains("lobe_definition") && lobeObj["lobe_definition"].isArray()) {
            jsonRoot["lobe_definition"] = lobeObj["lobe_definition"].toArray();
        }
    }
    
    // 添加其他字段，使用完整的任务绝对路径
    jsonRoot["path"] = m_taskAbsolutePath;      // 子任务数据保存路径
    jsonRoot["task_start_location"] = QString::fromStdString(goal->task_start_location);
    jsonRoot["task_end_location"] = QString::fromStdString(goal->task_end_location);
    
    // 转换为JSON字符串
    QJsonDocument jsonDoc(jsonRoot);
    QByteArray jsonData = jsonDoc.toJson(QJsonDocument::Compact);
    
    LOG_INFO("发送任务数据给YOLO子任务: {}", jsonData.toStdString());
    
    // 使用RequestManager发送请求到YOLO子任务
    auto callback = [this, task_id](const QByteArray& response) {
        LOG_INFO("YOLO任务数据发送完成，任务ID: {}，响应大小: {}", task_id, response.size());
    };
    
    m_requestManager->sendRequest(m_yoloSocket, RMS_TASK_START, jsonData, callback, 10000);
}

/**
 * @brief 处理喷药任务逻辑
 * 
 * @param task_id 任务ID
 * @param robotStatus 机器人状态
 * @param context 任务上下文
 */
void TaskServerNode::handleSprayingTask(const std::string& task_id,
                                       const rms_pkg::srv::RobotStatus::Response::SharedPtr& robotStatus,
                                       const TaskContext& context)
{
    // 只处理喷药任务
    if (!context.goal || context.goal->task_type != "spraying") {
        return;
    }
    
    // 棜查任务ID是否匹配
    if (context.task_id != task_id) {
        LOG_WARN("任务ID不匹配，当前任务: {}, 处理任务: {}", 
                    context.task_id, task_id);
        return;
    }
    
    QString currentTarget = QString::fromStdString(robotStatus->current_target);    // 当前目标站点
    QString lastStation = QString::fromStdString(robotStatus->last_station);        // 上一个站点
    QString taskStartLocation = QString::fromStdString(context.goal->task_start_location);
    QString taskEndLocation = QString::fromStdString(context.goal->task_end_location);
    LOG_INFO("lastStation: {}, currentTarget: {},  taskStartLocation: {}, taskEndLocation: {}",lastStation.toStdString(), currentTarget.toStdString(), taskStartLocation.toStdString(), taskEndLocation.toStdString());
    // 如果当前目标点为空，直接返回
    if (currentTarget.isEmpty()) {
        return;
    }
    
     // 更新目标点跟踪（只在目标点变化时更新）
    bool targetChanged = (currentTarget != m_previousTarget);
    if (targetChanged) {
        m_previousTarget = currentTarget;
    }
    
    // 检查是否已到达任务起始位置
    bool atOrPastStart = !taskStartLocation.isEmpty() && currentTarget >= taskStartLocation;
    
    // 检查是否已超过任务结束位置
    bool pastEnd = !taskEndLocation.isEmpty() && currentTarget > taskEndLocation;
    
    // 如果已超过结束位置，关闭喷药
    if (pastEnd) {
        if (m_isSpraying) {
            sendSprayingCommand(false);
            m_isSpraying = false;
        }
        LOG_INFO("喷药任务结束，已超过结束位置: {}", taskEndLocation.toStdString());
        return;
    }
    
    // 如果已到达起始位置，检查是否在陇区间内
    if (atOrPastStart) {
        // 判断行驶方向：只有当目标点变化时才判断方向
        // 如果lastStation < currentTarget，则为正向行驶
        bool isForward = true; // 默认正向
        if (!lastStation.isEmpty()) {
            isForward = lastStation < currentTarget;
        }
        
        // 获取当前路径对应的陇区间
        QString currentLobeKey = "";
        bool inLobeRange = false;
        
        if (!lastStation.isEmpty() && !currentTarget.isEmpty()) {
            std::tie(inLobeRange, currentLobeKey) = getLobeForPath(lastStation, currentTarget, context.goal->lobe_data);
        }
        
        // 只有在正向行驶且路径在陇区间范围内时才考虑开启喷洒
        if (isForward && inLobeRange) {
            // 检查这个陇区间是否已经被喷过药
            bool lobeAlreadySprayed = m_sprayedLobes.find(currentLobeKey.toStdString()) != m_sprayedLobes.end();
            
            if (!lobeAlreadySprayed && !m_isSpraying) {
                sendSprayingCommand(true);
                m_isSpraying = true;
                LOG_INFO("喷药任务进行中，正向行驶进入陇区间，路径: {}->{}，陇区间标识: {}", 
                         lastStation.toStdString(), currentTarget.toStdString(), currentLobeKey.toStdString());
            }
        } else {
            // 反向行驶或不在陇区间内，关闭喷洒
            if (m_isSpraying) {
                sendSprayingCommand(false);
                m_isSpraying = false;
                if (!isForward && inLobeRange) {
                    LOG_INFO("喷药任务进行中，反向行驶在陇区间内，路径: {}->{}，关闭喷洒", 
                             lastStation.toStdString(), currentTarget.toStdString());
                } else if (!inLobeRange) {
                    LOG_INFO("喷药任务进行中，路径不在陇区间内，路径: {}->{}，关闭喷洒", 
                             lastStation.toStdString(), currentTarget.toStdString());
                }
            }
        }
    } else {
        // 还未到达起始位置，关闭喷洒
        if (m_isSpraying) {
            sendSprayingCommand(false);
            m_isSpraying = false;
            LOG_INFO("喷药任务未到达起始位置，路径: {}->{}，关闭喷洒", 
                     lastStation.toStdString(), currentTarget.toStdString());
        }
    }
}


/**
 * @brief 获取路径对应的陇区间
 * 
 * @param lastStation 上一个站点
 * @param currentTarget 当前目标站点
 * @param lobeData 陇区间数据
 * @return std::pair<bool, QString> 第一个值表示路径是否在陇区间内，第二个值是陇区间标识
 */
std::pair<bool, QString> TaskServerNode::getLobeForPath(const QString& lastStation, const QString& currentTarget, const std::string& lobeData) {
    try {
        QJsonParseError parseError;
        QJsonDocument doc = QJsonDocument::fromJson(QString::fromStdString(lobeData).toUtf8(), &parseError);
        if (parseError.error != QJsonParseError::NoError) {
            LOG_ERROR("解析陇区间JSON数据失败: {}", parseError.errorString().toStdString());
            return std::make_pair(false, "");
        }
        
        QJsonArray lobeArray = doc.object()["lobe_definition"].toArray();
        
        // 遍历所有陇区间，检查当前路径是否在某个陇区间范围内
        for (const auto& lobeValue : lobeArray) {
            QJsonObject lobeObj = lobeValue.toObject();
            QString start = lobeObj["start"].toString();
            QString end = lobeObj["end"].toString();
            
            // 构造陇区间标识
            QString lobeKey = start + "-" + end;
            
            // 检查路径是否在陇区间范围内（包括边界）
            if (lastStation >= start && currentTarget <= end) {
                return std::make_pair(true, lobeKey);
            }
        }
    } catch (const std::exception& e) {
        LOG_ERROR("获取路径对应陇区间时发生异常: {}", e.what());
    }
    
    return std::make_pair(false, "");
}

/**
 * @brief 检查当前位置是否在陇区间内
 * 
 * @param currentTarget 当前目标位置
 * @param lobeData 陇区间数据
 * @return bool 是否在陇区间内
 */
bool TaskServerNode::isCurrentTargetInLobe(const QString& currentTarget, const std::string& lobeData)
{
    // 解析lobe_data中的lobe_definition
    QJsonParseError parseError;
    QJsonDocument lobeDoc = QJsonDocument::fromJson(lobeData.c_str(), &parseError);
    if (parseError.error != QJsonParseError::NoError || !lobeDoc.isObject()) {
        LOG_ERROR("解析lobe_data失败: {}", parseError.errorString().toStdString());
        return false;
    }
    
    QJsonObject lobeObj = lobeDoc.object();
    if (!lobeObj.contains("lobe_definition") || !lobeObj["lobe_definition"].isArray()) {
        LOG_ERROR("lobe_data中缺少lobe_definition字段或格式错误");
        return false;
    }
    
    QJsonArray lobeArray = lobeObj["lobe_definition"].toArray();
    
    // 遍历所有陇区间，检查当前位置是否在某个区间内
    for (const QJsonValue& value : lobeArray) {
        if (value.isObject()) {
            QJsonObject lobe = value.toObject();
            if (lobe.contains("start") && lobe.contains("end")) {
                QString start = lobe["start"].toString();
                QString end = lobe["end"].toString();
                
                // 检查currentTarget是否在[start, end]区间内
                if (!start.isEmpty() && !end.isEmpty() && 
                    currentTarget >= start && currentTarget <= end) {
                    return true;
                }
            }
        }
    }
    
    return false;
}

/**
 * @brief 发送喷药控制命令
 * 
 * @param enable 是否开启喷药
 */
void TaskServerNode::sendSprayingCommand(bool enable)
{
    static bool lastState = false;
    
    // 如果状态没有变化，则不发送命令
    if (lastState == enable) {
        return;
    }
    
    lastState = enable;
    
    // 构造JSON数据
    QJsonObject jsonRoot;
    jsonRoot["id"] = spraying_gpio_id_;             //SRC2000 DO的ID（DO_00)
    jsonRoot["status"] = enable;    //true 为高电平, false 为低电平
    
    // 转换为JSON字符串
    QJsonDocument jsonDoc(jsonRoot);
    QByteArray jsonData = jsonDoc.toJson(QJsonDocument::Compact);
    
    LOG_INFO("发送喷药控制GPIO:{} 命令: {}", spraying_gpio_id_, enable ? "开启" : "关闭");
    
    // 使用RequestManager发送请求到喷药子系统
    auto callback = [this, enable](const QByteArray& response) {
        LOG_INFO("喷药控制命令发送完成，状态: {}，响应大小: {}", 
                     enable ? "开启" : "关闭", response.size());
    };
    
    // 假设喷药控制命令使用命令ID 8002（需要在SCHeadData.h中定义）
    m_requestManager->sendRequest(m_otherSocket, CMD_OTHEAR_DO_SET, jsonData, callback, 10000);
}

/**
 * @brief 将任务状态字符串转换为对应的整数状态码
 * 
 * @param state 任务状态字符串，可能的值包括:
 *              - "NONE": 无状态
 *              - "RUNNING": 运行中
 *              - "SUSPENDED": 已暂停
 *              - "COMPLETED": 已完成
 *              - "FAILED": 已失败
 *              - "CANCELED": 已取消
 * @return 对应的状态码:
 *         - 0: NONE (无状态)
 *         - 2: RUNNING (运行中)
 *         - 3: SUSPENDED (已暂停)
 *         - 4: COMPLETED (已完成)
 *         - 5: FAILED (已失败)
 *         - 6: CANCELED (已取消)
 *         - -1: 未知状态
 */
int TaskServerNode::getTaskStatus(const QString& state)
{
    if (state == "NONE") return 0;
    if (state == "RUNNING") return 2;
    if (state == "SUSPENDED") return 3;
    if (state == "COMPLETED") return 4;
    if (state == "FAILED") return 5;
    if (state == "CANCELED") return 6;
    return -1; // UNKNOWN
}

/**
 * @brief 导出指定任务时间段内的传感器数据到CSV文件
 * 
 * 该函数从数据库中查询指定时间范围内的传感器数据，并将其导出为CSV文件。
 * 导出的传感器数据包括空气温湿度、光照强度、CO2浓度以及土壤温湿度等环境参数。
 * 
 * @param task_id 任务ID，用于创建任务目录和命名导出的CSV文件
 * @param start_time 数据导出的起始时间
 * @param end_time 数据导出的结束时间
 */
void TaskServerNode::exportSensorData(const std::string& task_id, rclcpp::Time start_time, rclcpp::Time end_time)
{
    LOG_INFO("Exporting sensor data for task: {}", task_id);
    
    // 将ROS时间转换为字符串格式 (使用QDateTime来处理)
    auto start_time_point = start_time.get_clock_type() == RCL_ROS_TIME ? 
        std::chrono::nanoseconds(start_time.nanoseconds()) : 
        std::chrono::nanoseconds(start_time.nanoseconds());
    
    auto end_time_point = end_time.get_clock_type() == RCL_ROS_TIME ? 
        std::chrono::nanoseconds(end_time.nanoseconds()) : 
        std::chrono::nanoseconds(end_time.nanoseconds());
    
    // 转换为 QDateTime
    QDateTime start_qdatetime = QDateTime::fromMSecsSinceEpoch(
        std::chrono::duration_cast<std::chrono::milliseconds>(start_time_point).count());
    QDateTime end_qdatetime = QDateTime::fromMSecsSinceEpoch(
        std::chrono::duration_cast<std::chrono::milliseconds>(end_time_point).count());
    
    QString start_time_str = start_qdatetime.toString("yyyy-MM-dd hh:mm:ss.zzz");
    QString end_time_str = end_qdatetime.toString("yyyy-MM-dd hh:mm:ss.zzz");
    
    // 查询传感器数据
    auto records = dbManager_->selectRecordsByTimestampRange("sensor_data", "timestamp", start_time_str, end_time_str);
    
    // 创建CSV文件
    QString csvFilePath = QString("data/task/%1/sensor_data.csv").arg(QString::fromStdString(task_id));
    std::ofstream csvFile(csvFilePath.toStdString());
    
    if (!csvFile.is_open()) {
        LOG_ERROR("Failed to create CSV file: {}", csvFilePath.toStdString());
        return;
    }
    
    // 写入CSV头部
    csvFile << "id,robot_id,timestamp,air_temperature,air_humidity,light_intensity,co2_concentration,soil_temperature,soil_moisture\n";
    
    // 写入数据
    for (const auto& record : records) {
        QVariantMap data = record.toMap();
        csvFile << data["id"].toInt() << ","
                << data["robot_id"].toString().toStdString() << ","
                << data["timestamp"].toString().toStdString() << ","
                << data["air_temperature"].toDouble() << ","
                << data["air_humidity"].toDouble() << ","
                << data["light_intensity"].toDouble() << ","
                << data["co2_concentration"].toInt() << ","
                << data["soil_temperature"].toDouble() << ","
                << data["soil_moisture"].toDouble() << "\n";
    }
    
    csvFile.close();
    LOG_INFO("Sensor data exported to: {}", csvFilePath.toStdString());
}

/**
 * @brief 将任务执行结果保存到数据库中
 * 
 * 该函数负责将指定任务的执行结果存储到SQLite数据库中。它会创建或更新task_results表中的记录，
 * 包括任务ID、状态、消息、完成百分比以及任务的开始和结束时间。
 * 
 * @param task_id 任务的唯一标识符
 * @param taskStatus 任务执行状态码 (4: 成功, 5: 失败, 6: 取消)
 * @param start_time 任务开始执行的时间
 * @param end_time 任务结束执行的时间
 */
void TaskServerNode::saveTaskResultToDatabase(const std::string& task_id, int taskStatus, 
                                              rclcpp::Time start_time, rclcpp::Time end_time)
{
    LOG_INFO("Saving task result to database for task: {}", task_id);
    
    // 准备状态和消息
    QString status, message;
    switch (taskStatus) {
        case 4:
            status = "succeeded";
            message = "Task completed successfully";
            break;
        case 5:
            status = "failed";
            message = "Task failed";
            break;
        case 6:
            status = "canceled";
            message = "Task canceled";
            break;
        default:
            status = "unknown";
            message = "Unknown task status";
            break;
    }
    
    // 将时间转换为字符串
    auto start_time_point = start_time.get_clock_type() == RCL_ROS_TIME ? 
        std::chrono::nanoseconds(start_time.nanoseconds()) : 
        std::chrono::nanoseconds(start_time.nanoseconds());
    
    auto end_time_point = end_time.get_clock_type() == RCL_ROS_TIME ? 
        std::chrono::nanoseconds(end_time.nanoseconds()) : 
        std::chrono::nanoseconds(end_time.nanoseconds());
    
    // 转换为 QDateTime
    QDateTime start_qdatetime = QDateTime::fromMSecsSinceEpoch(
        std::chrono::duration_cast<std::chrono::milliseconds>(start_time_point).count());
    QDateTime end_qdatetime = QDateTime::fromMSecsSinceEpoch(
        std::chrono::duration_cast<std::chrono::milliseconds>(end_time_point).count());
    
    QString start_time_str = start_qdatetime.toString("yyyy-MM-dd hh:mm:ss.zzz");
    QString end_time_str = end_qdatetime.toString("yyyy-MM-dd hh:mm:ss.zzz");
    
    // 插入或更新任务结果
    QVariantMap data;
    data["task_id"] = QString::fromStdString(task_id);
    data["status"] = status;
    data["message"] = message;
    data["completion_percentage"] = 100;
    data["start_time"] = start_time_str;
    data["end_time"] = end_time_str;
    
    if (!dbManager_->insertOrUpdate("task_results", data)) {
        LOG_ERROR("Failed to save task result to database: {}", dbManager_->lastError().toStdString());
    } else {
        LOG_INFO("Task result saved to database for task: {}", task_id);
    }
}
/**
 * @brief 创建任务目录
 * 
 * @param task_id 任务ID
 */
void TaskServerNode::createTaskDirectory(const std::string& task_id)
{
    QString taskDirPath = QString("data/task/%1").arg(QString::fromStdString(task_id));
    QDir taskDir(taskDirPath);
    if (!taskDir.exists()) {
        taskDir.mkpath(".");
        m_taskAbsolutePath = taskDir.absolutePath();    //当前任务绝对路径
        LOG_INFO("Created task directory: {} with absolute path: {}", 
                      taskDirPath.toStdString(), m_taskAbsolutePath.toStdString());
    }
}

/**
 * @brief 获取控制权限
 * 
 * 向机器人发送请求以获取控制权限。该函数会检查连接状态，
 * 构造请求消息，发送请求并处理响应结果。
 * 
 * @param nickName 用户昵称，用于标识请求方
 * @param cmdId 命令ID，用于标识本次请求
 */
void TaskServerNode::acquireControlAuthority(const QString& nickName, int cmdId) {
    // 检查连接状态
    if (!isConfConnected()) {
        LOG_WARN("抢占控制权失败: 机器人未连接");
        emit controlAuthorityAcquired(false);
        return;
    }
    
    // 构造请求JSON
    QJsonObject obj;
    obj["nick_name"] = nickName;
    QJsonDocument doc(obj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
    
    // 记录日志
    LOG_INFO("执行抢占控制权操作 - 命令ID: {}, 昵称: {}", cmdId, nickName.toStdString());
    
    // 发送抢占控制权请求，并设置回调函数处理响应
    m_requestManager->sendRequest(m_confSocket, cmdId, jsonData,
        [this, cmdId](const QByteArray &resp) {
            bool success = false;
            
            // 解析响应数据判断是否成功
            if (!resp.isEmpty()) {
                QJsonParseError parseError;
                QJsonDocument responseDoc = QJsonDocument::fromJson(resp, &parseError);
                if (parseError.error == QJsonParseError::NoError && responseDoc.isObject()) {
                    QJsonObject responseObject = responseDoc.object();
                    if (responseObject.contains("ret_code")) {
                        int retCode = responseObject["ret_code"].toInt(-1);
                        success = (retCode == 0);  // ret_code为0表示成功
                    }
                }
            }
            
            LOG_INFO("抢占控制权完成 - 命令ID: {}, 结果: {}", cmdId, success ? "成功" : "失败");
            
            // 如果抢占控制权成功，则更新hasControlAuthority标志
            if (success) {
                hasControlAuthority = true;
            }
            
            // 发出信号通知控制权获取结果
            emit controlAuthorityAcquired(success);
        }
    );
}

/**
 * @brief 处理控制权限获取结果的回调函数
 * 
 * 当机器人控制系统尝试获取控制权限的操作完成后，会调用此函数处理结果。
 * 如果获取成功，则继续执行任务；如果失败，则中止当前任务。
 * 
 * @param success 控制权限获取是否成功的布尔值
 *        - true: 获取成功，继续执行任务
 *        - false: 获取失败，中止任务执行
 * 
 * @note 函数使用局部变量保存任务上下文，避免在异步操作期间访问可能被修改的成员变量
 */
void TaskServerNode::onControlAuthorityAcquired(bool success)
{
    // 直接使用 m_currentTaskContext 而不是移动所有权
    if (!m_currentTaskContext) {
        LOG_WARN("收到控制权获取结果，但没有待处理的任务");
        return;
    }
    
    if (success) {
        LOG_INFO("控制权获取成功，继续执行任务");
        // 控制权获取成功，稍后执行任务，确保服务器处理完控制权请求
        QTimer::singleShot(100, this, [this]() {
            // 确保任务上下文仍然存在
            if (!m_currentTaskContext) {
                LOG_ERROR("任务上下文在延迟后丢失");
                return;
            }
            
            executeTaskCommand(
                m_currentTaskContext->goal,
                m_currentTaskContext->json_data,
                m_currentTaskContext->goal_handle,
                m_currentTaskContext->start_time);
        });
    } else {
        LOG_ERROR("获取控制权失败");
        // 控制权获取失败，中止任务
        auto result = std::make_shared<TaskExecution::Result>();
        result->task_id = m_currentTaskContext->goal->task_id;
        result->status = "failed";
        result->message = "Failed to acquire control authority";
        result->completion_percentage = 0;
        result->start_time = m_currentTaskContext->start_time;
        result->end_time = this->get_clock()->now();
        m_currentTaskContext->goal_handle->abort(result);
    }
}

/**
 * @brief 执行任务命令
 * 
 * 根据传入的命令ID执行相应的任务操作，支持暂停、恢复、取消和指定路径导航等命令
 * 
 * @param goal 包含任务命令信息的目标对象指针
 * @param jsonData 与命令相关的JSON格式数据
 * @param goal_handle 用于与客户端通信的目标句柄
 * @param start_time 任务开始时间
 */
void TaskServerNode::executeTaskCommand(
    const std::shared_ptr<const TaskExecution::Goal> goal,
    const QByteArray& jsonData,
    const std::shared_ptr<GoalHandle> goal_handle,
    const rclcpp::Time& start_time)
{
    LOG_INFO("Executing task command: {} jsonData: {}", goal->cmd_id, jsonData.toStdString());
    
    // 根据命令ID调用相应的处理函数（异步方式）
    switch(goal->cmd_id) {
        case CMD_NAV_PAUSE: // pause command
            sendPauseCommand(goal);
            break;
        case CMD_NAV_RESUME: // resume command
            sendResumeCommand(goal);
            break;
        case CMD_NAV_CANCEL: // cancel command
            sendCancelCommand(goal);
            break;
        case CMD_NAV_SPECIFY_PATH: // navigation command
            sendNavigationCommand(goal, jsonData);
            break;
        default:
            LOG_WARN("Unsupported cmd_id: {}. Only 3001/3002/3003/3066 (navigation) are supported.", goal->cmd_id);
            auto result = std::make_shared<TaskExecution::Result>();
            result->task_id = goal->task_id;
            result->status = "failed";
            result->message = "Unsupported cmd_id";
            result->completion_percentage = 0;
            result->start_time = start_time;
            result->end_time = this->get_clock()->now();
            goal_handle->abort(result);
            break;
    }
}

/**
 * @brief 处理任务命令发送结果的回调函数
 * 
 * 当任务命令发送完成后，根据发送结果更新任务状态并通知客户端
 * 
 * @param task_id 任务ID
 * @param success 命令发送是否成功
 * @param message 与结果相关的消息描述
 */
void TaskServerNode::onTaskCommandSent(const std::string& task_id, bool success, const std::string& message)
{
    // 直接使用 m_currentTaskContext 而不是移动所有权
    LOG_INFO("Received task command send result for task {}", task_id);
    // 检查task_id是否有效
    if (task_id.empty()) {
        LOG_ERROR("任务ID为空");
        return;
    }
    
    if (!m_currentTaskContext) {
        LOG_WARN("收到任务命令发送结果，但没有待处理的任务");
        return;
    }
    
    LOG_INFO("Received task command send result for task {}. Success: {}, Message: {}", task_id, success, message);
    auto result = std::make_shared<TaskExecution::Result>();
    result->task_id = task_id;
    
    // 根据命令发送结果更新任务状态
    if (success) {
        result->status = "executing";
        result->message = message;
        result->completion_percentage = 50;
        m_currentTaskContext->goal_handle->succeed(result);
        LOG_INFO("Task {} command sent.", task_id);
        // 任务成功发送后，可以清理任务上下文
        m_currentTaskContext.reset();
    } else {
        result->status = "failed";
        result->message = message;
        result->completion_percentage = 0;
        result->start_time = m_currentTaskContext->start_time;
        result->end_time = this->get_clock()->now();
        m_currentTaskContext->goal_handle->abort(result);
        LOG_ERROR("Task {} command failed.", task_id);
        // 任务失败后，清理任务上下文
        m_currentTaskContext.reset();
    }
}

/**
 * @brief 向机器人发送暂停命令
 * 
 * 该函数通过m_requestManager向机器人的导航套接字发送暂停命令，
 * 不需要附加JSON数据。发送后通过回调函数处理机器人的响应。
 * 
 * @param goal 包含命令ID等信息的目标对象
 */
void TaskServerNode::sendPauseCommand(const std::shared_ptr<const TaskExecution::Goal> goal)
{
    LOG_INFO("暂停: {}", goal->cmd_id);
    auto callback = [this, cmd_id = goal->cmd_id](const QByteArray& response) {
        handleRobotResponseForTask(cmd_id, response);
    };
    
    // 发送暂停命令，不带JSON数据
    m_requestManager->sendRequest(m_naviSocket, goal->cmd_id, QByteArray(), callback, 10000);
}

/**
 * @brief 向机器人发送恢复命令
 * 
 * 该函数通过m_requestManager向机器人的导航套接字发送恢复命令，
 * 不需要附加JSON数据。发送后通过回调函数处理机器人的响应。
 * 
 * @param goal 包含命令ID等信息的目标对象
 */
void TaskServerNode::sendResumeCommand(const std::shared_ptr<const TaskExecution::Goal> goal)
{
    LOG_INFO("恢复: {}", goal->cmd_id);
    auto callback = [this, cmd_id = goal->cmd_id](const QByteArray& response) {
        handleRobotResponseForTask(cmd_id, response);
    };
    
    // 发送恢复命令，不带JSON数据
    m_requestManager->sendRequest(m_naviSocket, goal->cmd_id, QByteArray(), callback, 10000);
}

/**
 * @brief 向机器人发送取消命令
 * 
 * 该函数通过m_requestManager向机器人的导航套接字发送取消命令，
 * 不需要附加JSON数据。发送后通过回调函数处理机器人的响应。
 * 
 * @param goal 包含命令ID等信息的目标对象
 */
void TaskServerNode::sendCancelCommand(const std::shared_ptr<const TaskExecution::Goal> goal)
{
    LOG_INFO("取消: {}", goal->cmd_id);
    auto callback = [this, cmd_id = goal->cmd_id](const QByteArray& response) {
        handleRobotResponseForTask(cmd_id, response);
    };
    
    // 发送取消命令，不带JSON数据
    m_requestManager->sendRequest(m_naviSocket, goal->cmd_id, QByteArray(), callback, 10000);
}

/**
 * @brief 向机器人发送导航命令
 * 
 * 该函数通过m_requestManager向机器人的导航套接字发送导航命令，
 * 需要附加JSON数据。为避免与控制权命令冲突，会在发送前延迟100毫秒。
 * 
 * @param goal 包含命令ID等信息的目标对象
 * @param jsonData 需要发送的JSON格式数据
 */
void TaskServerNode::sendNavigationCommand(const std::shared_ptr<const TaskExecution::Goal> goal, const QByteArray& jsonData)
{
    LOG_INFO("指定路径导航: {}", goal->cmd_id);
    
    // 确保任务上下文已设置
    if (!m_currentTaskContext) {
        LOG_WARN("发送导航命令时任务上下文为空");
        // 这里我们不创建新的上下文，因为应该已经存在了
        // 如果确实不存在，可能需要检查execute_task函数中的逻辑
    }
    
    auto callback = [this, cmd_id = goal->cmd_id](const QByteArray& response) {
        handleRobotResponseForTask(cmd_id, response);
    };
    
    // 延迟一小段时间再发送导航命令，避免与控制权命令冲突
    QTimer::singleShot(100, this, [this, goal, jsonData, callback]() {
        // 检查数据是否有效
        if (jsonData.isEmpty()) {
            LOG_WARN("空的JSON数据，使用空数据发送导航命令");
            m_requestManager->sendRequest(m_naviSocket, goal->cmd_id, QByteArray(), callback, 10000);
        } else {
            // 发送导航命令，带JSON数据
            m_requestManager->sendRequest(m_naviSocket, goal->cmd_id, jsonData, callback, 10000);
        }
    });
}

/**
 * @brief 处理来自机器人的任务命令响应
 * 
 * 该函数用于处理机器人对发送命令的响应，解析响应数据并根据命令类型和执行结果
 * 记录日志并发出相应的信号。主要处理导航相关的命令响应，如暂停、恢复、取消和指定路径导航等。
 * 
 * @param cmdId 命令ID，标识发送给机器人的命令类型
 * @param response 机器人返回的响应数据，通常为JSON格式
 */
void TaskServerNode::handleRobotResponseForTask(int cmdId, const QByteArray& response)
{
    LOG_INFO("收到机器人应答命令: {}", cmdId);
    
    // 直接使用 m_currentTaskContext 而不是移动所有权
    bool success = false;
    std::string message = "Unknown error";
    
    // 添加空指针检查
    if (response.isEmpty()) {
        LOG_WARN("Empty response received for cmd_id: {}", cmdId);
        success = false;
        message = "Empty response received";
    } else {
        // 解析并处理响应数据
        QJsonParseError parseError;
        QJsonDocument doc = QJsonDocument::fromJson(response, &parseError);
        
        if (parseError.error != QJsonParseError::NoError) {
            LOG_ERROR("Failed to parse robot response for cmd_id {}: {}", cmdId, parseError.errorString().toStdString());
            success = false;
            message = "Failed to parse robot response: " + parseError.errorString().toStdString();
        } else {
            LOG_DEBUG("Task response for cmd_id {} ", cmdId);
            
            QJsonObject responseObject = doc.object();
            int retCode = responseObject["ret_code"].toInt(-1);
            success = (retCode == 0);
            
            // 根据不同的命令ID处理响应
            switch (cmdId) {
                case CMD_NAV_PAUSE:
                    if (success) {
                        message = "Navigation pause command sent successfully";
                        LOG_INFO("导航暂停成功.");
                    } else {
                        message = "Navigation pause command failed";
                        LOG_INFO("导航暂停失败.");
                    }
                    break;
                case CMD_NAV_RESUME:
                    if (success) {
                        message = "Navigation resume command sent successfully";
                        LOG_INFO("导航恢复成功.");
                    } else {
                        message = "Navigation resume command failed";
                        LOG_INFO("导航恢复失败.");
                    }
                    break;
                case CMD_NAV_CANCEL:
                    if (success) {
                        message = "Navigation cancel command sent successfully";
                        LOG_INFO("导航取消成功."); 
                    } else {
                        message = "Navigation cancel command failed";
                        LOG_INFO("导航取消失败.");
                    }
                    break;
                case CMD_NAV_SPECIFY_PATH:
                    if (success) {
                        message = "Navigation command sent successfully";
                        LOG_INFO("指定路径导航任务接收成功.");
                        
                        // 如果是巡检任务，在导航命令发送成功后立即发送任务数据给YOLO子任务
                        if (m_currentTaskContext && m_currentTaskContext->goal && 
                            m_currentTaskContext->goal->task_type == "inspection") {
                            sendTaskStartToYolo(m_currentTaskContext->goal->task_id, m_currentTaskContext->goal);
                        }
                    } else {
                        message = "Navigation command failed";
                        LOG_INFO("指定路径导航任务接收失败.");
                    }
                    break;
                default:
                    if (success) {
                        message = "Command sent successfully";
                    } else {
                        message = "Command failed";
                    }
                    break;
            }
        }
    }
    LOG_INFO("响应结果: {}", message);
    
    // 发出信号通知任务命令发送结果
    try {
        std::string task_id = "unknown";
        if (m_currentTaskContext && m_currentTaskContext->goal) {
            task_id = m_currentTaskContext->goal->task_id;
        } else {
            // 尝试从m_taskContexts中查找任务
            for (const auto& pair : m_taskContexts) {
                if (pair.second.goal && pair.second.goal->cmd_id == cmdId) {
                    task_id = pair.second.goal->task_id;
                    LOG_DEBUG("在m_taskContexts中找到匹配的任务: {}",  task_id);
                    break;
                }
            }
        }
        emit taskCommandSent(task_id, success, message);
    } catch (const std::exception& e) {
        LOG_ERROR("发送任务命令完成信号时发生异常: {}", e.what());
    }
}

/**
 * @brief 向YOLO子任务发送位置更新信息
 * 
 * @param task_id 任务ID
 * @param currentLocation 当前位置
 * @param targetLocation 目标位置
 */
void TaskServerNode::sendLocationUpdateToYolo(const std::string& task_id,
                                             const QString& currentLocation,
                                             const QString& targetLocation)
{
    LOG_DEBUG("向YOLO子任务发送位置更新信息，任务ID: {}, 当前位置: {}, 目标位置: {}", 
                  task_id, currentLocation.toStdString(), targetLocation.toStdString());
    
    // 检查连接状态
    if (!isYoloConnected()) {
        LOG_WARN("YOLO socket未连接，无法发送位置更新信息");
        return;
    }
    
    // 构造JSON数据
    QJsonObject jsonRoot;
    jsonRoot["current_location"] = currentLocation;
    jsonRoot["target_location"] = targetLocation;
    
    // 转换为JSON字符串
    QJsonDocument jsonDoc(jsonRoot);
    QByteArray jsonData = jsonDoc.toJson(QJsonDocument::Compact);
    
    LOG_DEBUG("发送位置更新信息给YOLO子任务: {}", jsonData.toStdString());
    
    // 使用RequestManager发送请求到YOLO子任务
    auto callback = [this, task_id](const QByteArray& response) {
        LOG_DEBUG("YOLO位置更新信息发送完成，任务ID: {}，响应大小: {}", task_id, response.size());
    };
        
    // 使用位置更新命令ID（需要在SCHeadData.h中定义，这里假设为RMS_TASK_STATUS）
    m_requestManager->sendRequest(m_yoloSocket, RMS_TASK_STATUS, jsonData, callback, 10000);
}

/**
 * @brief 向YOLO子任务发送任务结束通知
 * 
 * @param task_id 任务ID
 */
void TaskServerNode::sendTaskEndToYolo(const std::string& task_id)
{
    LOG_DEBUG("向YOLO子任务发送任务结束通知，任务ID: {}", task_id);
    
    // 检查连接状态
    if (!isYoloConnected()) {
        LOG_WARN("YOLO socket未连接，无法发送任务结束通知");
        return;
    }
    
    // 使用RequestManager发送请求到YOLO子任务，不带JSON数据
    auto callback = [this, task_id](const QByteArray& response) {
        LOG_INFO("YOLO任务结束通知发送完成，任务ID: {}，响应大小: {}", task_id, response.size());
    };
    
    // 发送任务结束命令，不带JSON数据
    m_requestManager->sendRequest(m_yoloSocket, RMS_TASK_END, QByteArray(), callback, 10000);
}

/**
 * @brief 处理特定任务类型的清理工作
 * 
 * 根据任务类型执行相应的清理操作：
 * - 对于喷药任务(spraying)：发送关闭喷药设备指令
 * - 对于巡检任务(inspection)：向YOLO子任务发送任务结束信号
 * 
 * @param goal 指向任务目标的共享指针，包含任务类型和任务ID等信息
 */
void TaskServerNode::handleTaskTypeSpecificCleanup(const std::shared_ptr<const TaskExecution::Goal> goal)
{
    if (!goal) {
        LOG_ERROR("Invalid goal pointer");
        return;
    }

    // 如果是喷药任务，确保关闭喷药设备
    if (goal->task_type == "spraying") {
        sendSprayingCommand(false); // 关闭喷药
        LOG_INFO("Task {} ended, spraying stopped", goal->task_id);
    }

    // 如果是巡检任务，向YOLO子任务发送任务结束信号
    if (goal->task_type == "inspection") {
        sendTaskEndToYolo(goal->task_id);
    }
}

int main(int argc, char ** argv)
{
    // 初始化全局日志记录器
    GlobalLogger::getInstance().initialize("data/logs", "RMSClient_task_server", 15);
    
    // 初始化Qt应用
    QCoreApplication app(argc, argv);
    
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TaskServerNode>();
    
    // 使用单线程执行器处理ROS 2回调
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    
    // 创建一个QTimer来定期调用executor的spin_some
    QTimer timer;
    timer.setInterval(10); // 10ms
    QObject::connect(&timer, &QTimer::timeout, [&executor]() {
        executor.spin_some();
    });
    timer.start();
    
    // Qt事件循环在主线程中运行，同时定期处理ROS 2回调
    int result = app.exec();
    
    rclcpp::shutdown();
    return result;
}

#include "task_action_server.moc"